#include "hcfa.h"
#include "../../bsp/cli_cmd.h"
#include "../../bsp/io_config.h"

static int __local_assert(xp_motor_t *motor)
{
    if (motor->inited != 1 ||
        motor->private_data == NULL ||
        motor->ops == NULL)
    {
        println("motor not inited.");
        return -1;
    }
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    if (mb_dev->bus == NULL)
    {
        println("motor %s not mount in bus of modbus", mb_dev->name);
        return -2;
    }
    return 0;
}

#define LOCAL_ASSERT(motor)    \
    if (__local_assert(motor)) \
    return -1

static int hcfa_modbus_config(struct xp_modbus_device *dev, u8 addr_group, u8 addr_sub, u16 value)
{
    struct xp_modbus_bus *bus = dev->bus;
    if (dev == NULL || bus == NULL || bus->swrite == NULL) return -1;
    u16 addr = addr_group;
    addr <<= 8;
    addr += addr_sub;
    if (0 != bus->swrite(dev, addr, value)) {
        println("hcfa_modbus_config addr:%04X value:%04X faild.", addr, value);
        return -2;
    }
    aos_msleep(20);
}

/* 电机初始化配置
 * 对于从机地址与通信波特率应该在装机时配置
 */
static int hcfa_init(xp_motor_t *motor)
{
    // 配置电机加速度、减速度、使能IO等
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    // 根据配置信息，设定为速度模式或位置模式
    int ret = hcfa_modbus_config(mb_dev, 0, 1, 1);     // 使用速度模式
    ret |= hcfa_modbus_config(mb_dev, 3,  0, 0);       // 速度指令来源设为数字给定
    ret |= hcfa_modbus_config(mb_dev, 3,  3, 0);       // 设定给定速度为0
    ret |= hcfa_modbus_config(mb_dev, 3, 14, motor_attr->acc == 0 ? 500 : motor_attr->acc);// 设定加速时间为acc ms/1000rpm
    ret |= hcfa_modbus_config(mb_dev, 3, 15, motor_attr->dec == 0 ? 500 : motor_attr->dec);// 设定减速时间为dec ms/1000rpm
    ret |= hcfa_modbus_config(mb_dev, 4,  1, 1);       // 设定DI1用作使能
    ret |= hcfa_modbus_config(mb_dev, 4,  2, 23);      // 设定DI2用作电机转向
    ret |= hcfa_modbus_config(mb_dev, 4,  3, 30);      // 设定DI3用作电机快速停止
    ret |= hcfa_modbus_config(mb_dev, 4, 21, 3);       // 设定DO1用作报警输出
    ret |= hcfa_modbus_config(mb_dev, 4, 22, 2);       // 设定DO2用作错误输出

    if (ret != 0) {
        println("hcfa motor init faild.sid=%d.", mb_dev->sid);
        return -1;
    }

    return 0;
}

static int hcfa_speed_set(xp_motor_t *motor, motor_rpm_t rpm)
{
    LOCAL_ASSERT(motor);

    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    // 如果反方向需要写入补码
    unsigned short speed_addr = 0x0303;
    short speed = (rpm > 9000) ? 9000 : rpm;
    if (motor_attr->dir == motor_reverser) speed = 0-speed;
    int ret = bus->swrite(mb_dev, speed_addr, speed);
    if (0 == ret)
        motor_attr->rpm = rpm;

    if (0 != ret) println("hcfa_speed_set faild");

    return ret;
}

static int hcfa_speed_get(xp_motor_t *motor, motor_rpm_t *rpm)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    // 读取数值小于0，取源码，设置方向
    short speed = 0;
    unsigned short speed_addr = 0x0303;
    int ret = bus->sread(mb_dev, speed_addr, (unsigned short *)&speed);
    if (speed < 0) {
        *rpm = 0-speed;
        motor_attr->rpm = 0-speed;
        motor_attr->dir = motor_reverser;
    } else {
        *rpm = speed;
        motor_attr->rpm = speed;
        motor_attr->dir = motor_forward;
    }

    return (ret == 0)?(0):(-1);
}

static int hcfa_direction_set(xp_motor_t *motor, motor_direction_t dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    short speed = motor_attr->rpm;
    if (motor_attr->dir != dir) motor_attr->dir = dir;
    int ret = hcfa_speed_set(motor, motor_attr->rpm);
    if (ret != 0) {
        motor_attr->dir = (dir == motor_forward) ? motor_reverser : motor_forward;
        return -1;
    }
#if 0
    unsigned short _dir = (dir == motor_forward) ? 0 : 1;
    xp_io_write_pin(1, motor_attr->dir_io, (_dir == 0) ? 1 : 0);
    motor_attr->dir = _dir;
#endif

    return 0;
}

static int hcfa_direction_get(xp_motor_t *motor, motor_direction_t *dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    *dir = motor_attr->dir;

    return 0;
}

static int hcfa_start(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    hcfa_error_clear(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->en_io, 0);

    return 0;
}

static int hcfa_stop(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->en_io, 1);

    return 0;
}

static int hcfa_emerg_enable(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->emerg_io, 0);

    return 0;
}

static int hcfa_emerg_disable(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->emerg_io, 1);

    return 0;
}

static int hcfa_error_get(xp_motor_t *motor, motor_error_t *err_code)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short error_addr = (21 << 8) + 41;

    int ret = bus->sread(mb_dev, error_addr, err_code);

    return ret;
}

static int hcfa_error_clear(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short error = 1;
    unsigned short error_addr = (20 << 8) + 1;

    int ret = bus->swrite(mb_dev, error_addr, error);

    return ret;
}

static int hcfa_null_function(xp_motor_t *motor)
{
    return 0;
}

static int hcfa_encode_get(xp_motor_t *motor, int *num_of_turn, int *sub_angle)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    // 获取圈数 21.32
    union {
        char ch[4];
        int i;
    } read, tmp;
    int ret = bus->read(mb_dev, 0x1520, &read.i, 2, mb_read_single_reg);
    // 获取一圈的偏移 21.34
    tmp.ch[0] = read.ch[1];
    tmp.ch[1] = read.ch[0];
    tmp.ch[2] = read.ch[3];
    tmp.ch[3] = read.ch[2];
    *num_of_turn = tmp.i;
    int angle_pluse = 0;
    aos_msleep(20);
    ret |= bus->read(mb_dev, 0x1522, &read.i, 2, mb_read_single_reg);
    if (ret != 0) {
        return -1;
    }
    tmp.ch[0] = read.ch[1];
    tmp.ch[1] = read.ch[0];
    tmp.ch[2] = read.ch[3];
    tmp.ch[3] = read.ch[2];
    angle_pluse = tmp.i;
    *sub_angle = (angle_pluse * 360) / 131072;
    return 0;
}

static int hcfa_position_mode_init(xp_motor_t *motor)
{
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    // 设置位置模式，设置内部指令模式
    int ret = hcfa_modbus_config(mb_dev, 0, 1, 0);    
    ret |= hcfa_modbus_config(mb_dev, 0,  5, 2);       // 位置指令来源设为内部指令
    ret |= hcfa_modbus_config(mb_dev, 8,  9, motor_attr->acc == 0 ? 500 : motor_attr->acc);// 设定加速时间为acc ms/1000rpm
    ret |= hcfa_modbus_config(mb_dev, 4,  1, 1);       // 设定DI1用作使能
    ret |= hcfa_modbus_config(mb_dev, 4,  2, 25);      // 设定DI2用作位置使能
    ret |= hcfa_modbus_config(mb_dev, 4,  3, 30);      // 设定DI3用作电机快速停止
    ret |= hcfa_modbus_config(mb_dev, 4, 21, 3);       // 设定DO1用作报警输出
    ret |= hcfa_modbus_config(mb_dev, 4, 22, 2);       // 设定DO2用作错误输出
    // 设置第一段速
    ret |= hcfa_modbus_config(mb_dev, 8,  1, 1);
    ret |= hcfa_modbus_config(mb_dev, 8,  2, 1);
    //ret |= hcfa_modbus_config(mb_dev, 8,  8, motor_attr->rpm);
    
    if (ret != 0) {
        println("hcfa motor init faild.sid=%d.", mb_dev->sid);
        return -1;
    }
    return 0;
}

// 设置位置模式下的最大速度
static int hcfa_position_mode_speed_set(xp_motor_t *motor, motor_rpm_t rpm)
{
    LOCAL_ASSERT(motor);

    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    // 如果反方向需要写入补码
    unsigned short speed_addr = 0x0808;
    short speed = (rpm > 9000) ? 9000 : rpm;
    int ret = bus->swrite(mb_dev, speed_addr, speed);
    if (0 == ret)
        motor_attr->rpm = rpm;

    if (0 != ret) println("hcfa_speed_set faild");

    return ret;
}

// 获取位置模式下的最大速度
static int hcfa_position_mode_speed_get(xp_motor_t *motor, motor_rpm_t *rpm)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    short speed = 0;
    unsigned short speed_addr = 0x0808;
    int ret = bus->sread(mb_dev, speed_addr, (unsigned short *)&speed);
    if (ret != 0) {
        return -1;
    }
    *rpm = speed;
    motor_attr->rpm = speed;

    return 0;
}

// 位置模式下的方向设置
static int hcfa_position_mode_direction_set(xp_motor_t *motor, motor_direction_t dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    short speed = motor_attr->rpm;
    if (motor_attr->dir != dir) motor_attr->dir = dir;
    int pluse = motor_attr->pluse;
    if (dir == motor_reverser) {
        pluse = 0 - pluse;
    }
    int ret = bus->write(mb_dev, 0x0806, &pluse, 2, mb_write_mult_reg);
    if (ret != 0) {
        motor_attr->dir = (dir == motor_forward) ? motor_reverser : motor_forward;
        return -1;
    }

    return 0;
}

// 获取位置模式下的方向
static int hcfa_position_mode_direction_get(xp_motor_t *motor, motor_direction_t *dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    *dir = motor_attr->dir;

    return 0;
}

// 位置模式启动运行
static int hcfa_position_mode_start(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    //hcfa_error_clear(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->position_en_io, 0);
    aos_msleep(200);
    xp_io_write_pin(1, motor_attr->en_io, 0);

    return 0;
}

// 位置模式停止运行
static int hcfa_position_mode_stop(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    xp_io_write_pin(1, motor_attr->position_en_io, 1);
    aos_msleep(20);
    xp_io_write_pin(1, motor_attr->en_io, 1);

    return 0;
}

// 设置转动距离
static int hcfa_position_set(xp_motor_t *motor, int num_of_turn, int sub_angle)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct hcfa_motor_attr *motor_attr = (struct hcfa_motor_attr *)(mb_dev->device_attr);

    sub_angle %= 360;
    int pluse = num_of_turn * 10000 + (sub_angle*10000) / 360;
    motor_attr->pluse = pluse;
    if (motor_attr->dir == motor_reverser) {
        pluse = 0 - pluse;
    }

    // 写入
    int ret = bus->write(mb_dev, 0x0806, &pluse, 2, mb_write_mult_reg);
    if (ret != 0) {
        return -1;
    }

    return 0;
}

struct xp_motor_stdops hcfa_speed_stdops = {
    .init = hcfa_init,
    .speed_set = hcfa_speed_set,
    .speed_get = hcfa_speed_get,
    .direction_set = hcfa_direction_set,
    .direction_get = hcfa_direction_get,
    .rotation_start = hcfa_start,
    .rotation_stop = hcfa_stop,
    .emerg_enable = hcfa_emerg_enable,
    .emerg_disable = hcfa_emerg_disable,
    .error_get = hcfa_error_get,
    .error_clear = hcfa_error_clear,
    .encode_get = hcfa_encode_get,
    .position_set = hcfa_null_function,
};

struct xp_motor_stdops hcfa_position_stdops = {
    .init = hcfa_position_mode_init,
    .speed_set = hcfa_position_mode_speed_set,
    .speed_get = hcfa_position_mode_speed_get,
    .direction_set = hcfa_position_mode_direction_set,
    .direction_get = hcfa_position_mode_direction_get,
    .rotation_start = hcfa_position_mode_start,
    .rotation_stop = hcfa_position_mode_stop,
    .emerg_enable = hcfa_emerg_enable,
    .emerg_disable = hcfa_emerg_disable,
    .error_get = hcfa_error_get,
    .error_clear = hcfa_error_clear,
    .encode_get = hcfa_encode_get,
    .position_set = hcfa_position_set,
};


int xp_hcfa_speed_motor_constructor(struct xp_motor *motor, struct xp_modbus_device *dev, unsigned char *name)
{
    if (motor == NULL || dev == NULL || name == NULL)
        return -1;
    motor->private_data = (void *)dev;
    strncpy((char *)motor->name, (char *)name, XP_MOTOR_NAME_LENGTH);
    motor->ops = &hcfa_speed_stdops;

    if (NULL == motor->ops->init || 0 != motor->ops->init(motor))
    {
        motor->inited = 0;
        return -1;
    }

    motor->inited = 1;
    return 0;
}

int xp_hcfa_position_motor_constructor(struct xp_motor *motor, struct xp_modbus_device *dev, unsigned char *name)
{
    if (motor == NULL || dev == NULL || name == NULL)
        return -1;
    motor->private_data = (void *)dev;
    strncpy((char *)motor->name, (char *)name, XP_MOTOR_NAME_LENGTH);
    motor->ops = &hcfa_position_stdops;

    if (NULL == motor->ops->init || 0 != motor->ops->init(motor))
    {
        motor->inited = 0;
        return -1;
    }

    motor->inited = 1;
    return 0;
}


/*********** DEBUG TEST ************/
//#if (XP_CONFIG_AGV_WHEEL_DMKE_ENABLE == 1)
struct xp_motor hcfa_debug_motor_1 = {0};
struct xp_motor hcfa_debug_motor_2 = {0};

#include "../../driver/common/device_manager.h"
int xp_hcfa_motor_debug(char *type, char *fun, char *param)
{
    if (strcmp(type, "hcfa-1") != 0 &&
        strcmp(type, "hcfa-2") != 0)
        return 0;

    int ret = 0;
    static int debug_inited = 0;

    struct xp_modbus_device *hcfa_debug_dev_1 = get_modbus_device_by_name((unsigned char *)"hcfa_leftwheel");
    struct xp_modbus_device *hcfa_debug_dev_2 = get_modbus_device_by_name((unsigned char *)"hcfa_rightwheel");

    if (hcfa_debug_dev_1 == NULL || hcfa_debug_dev_2 == NULL)
        return 0;

    if (hcfa_debug_dev_1->inited != xp_true || hcfa_debug_dev_2->inited != xp_true)
        return 0;

    if (!debug_inited) {
        debug_inited = 1;
        //ret = xp_hcfa_motor_constructor(&hcfa_debug_motor_1, hcfa_debug_dev_1, (unsigned char *)"hcfa_debug_1");
        //ret |= xp_hcfa_motor_constructor(&hcfa_debug_motor_2, hcfa_debug_dev_2, (unsigned char *)"hcfa_debug_2");
        ret = xp_hcfa_position_motor_constructor(&hcfa_debug_motor_1, hcfa_debug_dev_1, (unsigned char *)"hcfa_debug_1");
        if (ret != 0)
            return 0;
    }
    motor_direction_t dir = motor_forward;
    int id = 0;

    char *p = strstr(type, "-");
    if (p == NULL)
        return 0;
    p++;
    id = atoi(p);

    struct xp_motor hcfa_debug_motor = {0};

    if (id == 1)
        hcfa_debug_motor = hcfa_debug_motor_1;
    else if (id == 2)
        hcfa_debug_motor = hcfa_debug_motor_2;
    else
        return 0;

    if (strcmp(fun, "set_speed") == 0)
    {
        println("set motor rpm = %d", atoi(param));
        ret = hcfa_debug_motor.ops->speed_set(&hcfa_debug_motor, atoi(param));
    }
    else if (strcmp(fun, "set_position") == 0)
    {
        char *p = strstr(param, "-");
        ret = hcfa_debug_motor.ops->position_set(&hcfa_debug_motor, atoi(param), (atoi(++p) >= 360 || atoi(p) < 0) ? 0 : atoi(p));
    }
    else if (strcmp(fun, "reset") == 0)
    {
        debug_inited = 0;
    }
    else if (strcmp(fun, "set_dir") == 0)
    {
        if (atoi(param) == 1)
            dir = motor_reverser;
        println("set motor dir = %d", dir);
        ret = hcfa_debug_motor.ops->direction_set(&hcfa_debug_motor, dir);
    }
    else if (strcmp(fun, "start") == 0)
    {
        println("set motor start");
        ret = hcfa_debug_motor.ops->rotation_start(&hcfa_debug_motor);
    }
    else if (strcmp(fun, "stop") == 0)
    {
        println("set motor stop");
        ret = hcfa_debug_motor.ops->rotation_stop(&hcfa_debug_motor);
    }
    else if (strcmp(fun, "emerg") == 0)
    {
        println("set motor emerg");
        ret = hcfa_debug_motor.ops->emerg_enable(&hcfa_debug_motor);
    }
    else if (strcmp(fun, "emerg_release") == 0)
    {
        println("set motor emerg");
        ret = hcfa_debug_motor.ops->emerg_disable(&hcfa_debug_motor);
    }

    return 1;
}
